﻿using System;
using System.Collections.Generic;
using System.Text;
using Cambia.CoreLib;
using LocalizationTest.Tools;
using System.Drawing;

namespace LocalizationTest
{
  class LandmarkDetectionSensor : Sensor
  {
    MapWithLandmarks.LandmarkIDs landmarkID;
    double headingDirection;
    double angleOfView;

    public LandmarkDetectionSensor()
    {
      this.landmarkID = MapWithLandmarks.LandmarkIDs.centerLeftFlag;
      this.headingDirection = 0;
      this.angleOfView = Math.PI / 2.0;
    }
    public LandmarkDetectionSensor(MapWithLandmarks.LandmarkIDs landmarkID, double headingDirection, double angleOfView)
    {
      this.landmarkID = landmarkID;
      this.headingDirection = headingDirection;
      this.angleOfView = angleOfView;
    }
    public override Measurement PredictForSimulation(WorldState worldState, Map map)
    {
      if (Globals.selfLocatorProperties.SensorNoise == NoiseType.noNoise)
        return PredictPerfect(worldState, map);
      else
      {
        Measurement perfectMeasurement = PredictPerfect(worldState, map);
        Measurement noisyMeasurementTrue = new DetectorMeasurement(true, this);
        Measurement noisyMeasurementFalse = new DetectorMeasurement(false, this);
        double pDetectDoor = GetMeasurementProbability(noisyMeasurementTrue, perfectMeasurement);
        double pNotDetectDoor = GetMeasurementProbability(noisyMeasurementFalse, perfectMeasurement);

        Random random = new Random();
        double rnd = random.NextDouble();

        if (rnd < pDetectDoor) return noisyMeasurementTrue;
        else return noisyMeasurementFalse;
      }
    }
    public override Measurement PredictPerfect(WorldState worldState, Map map)
    {
      double angle = 0;
      
      double x_robot = 0;
      double y_robot = 0;
      double theta_robot = 0;
      double x_landmark = 0;
      double y_landmark = 0;

      if (worldState is WorldState1D)
      {
        x_robot = (worldState as WorldState1D).Position;
      }

      if (worldState is WorldState2D)
      {
        x_robot = (worldState as WorldState2D).Vector.x;
        y_robot = (worldState as WorldState2D).Vector.y;
      }

      if (map is MapWithLandmarks)
      {
        x_landmark = (map as MapWithLandmarks).XPositionOfLandmark[(int)landmarkID];
        y_landmark = (map as MapWithLandmarks).YPositionOfLandmark[(int)landmarkID];
      }
      if (map is MapWithSingleLandmark)
      {
        x_landmark = (map as MapWithSingleLandmark).XPositionOfLandmark[(int)landmarkID];
        y_landmark = (map as MapWithSingleLandmark).YPositionOfLandmark[(int)landmarkID];
      }

      angle = Math.Atan2(y_landmark - y_robot, x_landmark - x_robot) - theta_robot;

      if (Math.Abs(MathTools.Normalize(angle - headingDirection)) < angleOfView / 2.0) 
        return new DetectorMeasurement(true, this);
      else
        return new DetectorMeasurement(false, this);
    }

    private double GetMeasurementProbability(Measurement noisyMeasurement, Measurement perfectMeasurement)
    {
      double probability = 0.0;
      bool noisy = (noisyMeasurement as DetectorMeasurement).SomethingWasDetected;
      bool perfect = (perfectMeasurement as DetectorMeasurement).SomethingWasDetected;

      if (noisy == true && perfect == true) probability = 0.95;
      if (noisy == false && perfect == true) probability = 0.05;
      if (noisy == true && perfect == false) probability = 0.3;
      if (noisy == false && perfect == false) probability = 0.7;
      return probability;
    }
    public override double GetMeasurementProbability(WorldState worldState, Map map, Measurement measurement)
    {
      Measurement perfectMeasurement = PredictPerfect(worldState, map);
      Measurement noisyMeasurement = measurement;
      double probability = GetMeasurementProbability(noisyMeasurement, perfectMeasurement);
      return probability;
    }

    /*
    public override void Draw(System.Drawing.Graphics graphics, Measurement measurement, WorldState worldState, double shade)
    {
      double startX = 0;
      double startY = 0;
      double endX = 0;
      double endY = 0;
      double endX2 = 0;
      double endY2 = 0;
      bool somethingWasDetected = (measurement as DetectorMeasurement).SomethingWasDetected;
      if (worldState is WorldState1D)
      {
        startX = (worldState as WorldState1D).Position;
        startY = 0;
      }
      if (worldState is WorldState2D)
      {
        startX = (worldState as WorldState2D).Vector.x;
        startY = (worldState as WorldState2D).Vector.y;
      }
      endX = startX + Math.Cos(headingDirection + angleOfView / 2.0) * 50.0;
      endY = startY + Math.Sin(headingDirection + angleOfView / 2.0) * 50.0;

      endX2 = startX + Math.Cos(headingDirection - angleOfView / 2.0) * 50.0;
      endY2 = startY + Math.Sin(headingDirection - angleOfView / 2.0) * 50.0;

      Color color = MapWithLandmarks.GetColorOfLandmark(landmarkID);
      Pen pen;
      if (shade == 0.9999)
        pen = new Pen(color, 1);
      else
        pen = new Pen(color, 3);
      graphics.DrawLine(pen, (float)startX, (float)startY, (float)endX, (float)endY);
      graphics.DrawLine(pen, (float)startX, (float)startY, (float)endX2, (float)endY2);

      SolidBrush solidBrush;
      if (somethingWasDetected)
        solidBrush = new SolidBrush(color);
      else
        solidBrush = new SolidBrush(Color.FromArgb(128, 128, 128, 128));
      graphics.FillPie(solidBrush,
        (float)startX - 25, (float)startY - 25,
        50, 50,
//        -45, 45);
        (float)( (headingDirection - angleOfView / 2.0) * 180.0 / Math.PI),
        (float)( angleOfView * 180.0 / Math.PI));
    }
    */
    public override void Draw(Measurement measurement, WorldState worldState, double shade, string drawingID)
    {
      //TODO
      throw new Exception("The method or operation is not implemented.");
    }

    public override string ToString()
    {
      throw new Exception("The method or operation is not implemented.");
    }
  }
}
